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Abstract 


One of the problems associated with redundant manipulators which have been proposed for space 
applications is that the reactions transmitted to the base of the manipulator as a result of the motion of the 
manipulator will cause undesirable effects on the dynamic behavior of the supporting space structure. It is 
therefore necessary to minimize the magnitudes of the forces and moments transmiiied to the base. In 
this report we show that kinematic redundancy can be used to solve the dynamic problem of minimizing 
the magnitude of the base reactions. The methodology described in the report is applied to a four 
degree-of-freedom spatial manipulator with one redundant degree-of-freedom. 




1.0 Introduction 

In the near future, autonomous robotic manipulators will be used on space stations to perform exhaustive 
tasks such as repairing of the exterior of a space station [1], and performing delicate experiments, etc. The 
employment of these manipulators will reduce extravehicular activity of the astronauts and free them for 
other tasks. 

Manipulators used in space are operated under microgravity conditions. Furthermore such manipulators 
will in general have redundant degrees of freedom in order to facilitate the peformance of tasks. (It is well 
known that redundant manipulators can be used to avoid obstacles, avoid singular configurai.ons, etc. (4]). 
In this research we are concerned with planning trajectories for redundant manipulators operating under 
microgravity conditions in space. 

The forces and moments transmitted by such "space" manipulators to the supporting structure will, in 
general, act as a disturbance on the spacecraft and therefore have an undesirable effect on the dynamic 
behavior of the spacecraft. Compensating for the disturbance (caused by the transmitted forces and 
moments) by means of a suitable control scheme is extremely difficult and expensive. An alternative 
approach, described in this report, is to plan a trajectory which minimizes the magnitude of the forces and 
moments transmitted by the manipulator to the supporting structure. (We will refer to these forces and 
moments as the base reactions.) 

The trajectory-planning problem for a manipulator reduces to the problem of solving the inverse-kinematic 
problem for the joint variables given the trajectory in the task-space. In the case of a non-redundant 
manipulator, this inverse-kinematic problem has, in general, a unique solution whereas in the redundant 
case, there are an infinite number of solutions to the inverse-kinematic problem. In this paper we pose an 
optimization problem of minimizing the base reactions in order to obtain a unique solution to the 
inverse-kinematic problem. 

Since there is a large body of research -see, for example, [2, 3, 4, 5, 6] - it is important at the outset to indicate 
the contributions of the current work. In contrast to most of the research on redundant manipulators, which 
has focused on the kinematics of these devices (2,4,5], we show how kinomatic redundancy can be used 






to resolve dynamic issues. In particular, we develop and apply a methodology for using kinematic 

redundancy to minimize the magnitude of the base reactions generated by the motion of the manipulator in 
space. 

2.0 Outline of Contents 

The paper is organized as follows. In the next section, we formally define the problem of minimizing the 
magnitude of the base reactions. The basic approach to solving the problem is defined in Section 4. The 
approach consists of two parts: 

1 . generation of the end-effector trajectory (Section 5) 

2. solution of the inverse-kinematic problem for the joint trajectories (Section 6). 

In Section 6 we pose the optimization problem of minimizing the magnitude of the base reactions in order 
to obtain the unique inverse-kinematic solution. The algorithm, used to implement the methodology 
developed in Sections 5 and 6, is described in Section 7. The application of the methodology to 
minimizing the base reactions of a four degree-of-freedom spatial manipulator (with one redundant 

degree-of-freedom) is demonstrated in Section 8. Finally, in the last section, we draw some conclusions 
and summarize our work. 

3.0 Problem Statement 

In this section we describe the trajectory planning problem for redundant manipulators. Consider a m 
degrees of freedom redundant manipulator with m revolute or prismatic joints mounted to a base. The 
base of the manipulator can be considered as part of the spacecraft or space station. 

In general let q, denote the joint displacement of joint i. The joint variable of each joint is either an angle 0j 
(revolute joint) or a distance dj (prismatic joint). The joint displacement vector q can be defined as 

*1“ I<1l. 02. .qm) T 

Let E be an arbitrary point on the end effector as shown in Figure 1 . The position of E can be represented 
with reference to the coordinate frame xyz fixed in the base by 
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The kinematic equations relating the end-effector position and the joint displacement variables are given 
by 


i 

I 
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i 


c. 


r(t) - g (q(t)) W 

where g is the vector of functions which are nonlinear in q(t) 

For a redundant manipulator, the number degrees of freedom, m is more than the minimum number 
degrees cf freedom required to perform a task which is denoted by n. Therefore, there are an infinite 
number of joint space solutions q(t) for a specified end-effector position, r(t). From these solutions, we 
can select a joint space solution based on certain criteria. In this paper we use the minimization of the base 
reactions as the criterion to select appropriate joint space solution. 

The trajectory planning problem studied in this paper is restricted to point-to-point motion. In a 
point-to-point motion, the objective is to move the end-effector of a manipulator from a starting position, fj 
to a final position, rj. During the execution of point-to-point motion, the base force (F 0 ) and base moment 
(N 0 ) exerted by the manipulator on the base cause undesired linear and angular motions of the supporting 
structures. Ideally, one would desire the base reactions to be zero. Since in reality this objective may not 
be achievable , we seek instead to minimize the magnitude o. the base reactions. 

The trajectory planning problem tor redundant manipulators which we are going to address has two 
requirements : (1) to move a redundant manipulator according to specified motion requirements; (2) ’o 
minimize the magnitude of the base reactions ( N 0 and F 0 ) transmitted by the manipulator to the base 

during motion. 

4.0 Description of the Approach 

The basic approach to the trajectory planning problem was described in [7,8] and consists of splitting the 
problem into two parts which enables us to deal with the end-effector trajectory and joint trajectories 
separately. 

The first part, described in Section 5, deals with the generation of the end-effector trajectory to satisfy 
certain motion constraints. The motion constraints of interest are the maximum acceleration of the 
end-effector trajectory and the total time of the task. 


| 
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In secto" 8 we discuss ,he second pad o, me approacn. The basic idea is ,o pose me inverse kinematics 
problem for determining the joint trajectories as an nntimi- 7 a *;, w 

of reZT ,eaal0nS TlK aPP, °“ h beSinS by pami0nin9 ,he i ° im "«*>,. , into a set 

l 'T T Va,iabl6S and a Sal 01 **« ** variabfes. This enabies us fo worn w„h 

^uare Jacobian matrix as in the nonredundan, manipuiato, case. Then me Jacobian matrix is paditioned 
.nto a nonsingular square Jacobian matrix associated with nonredundan, join, variabtes and a subma, 
associated wdh redundant join, variables. Using these paditioned matrices, we are abie to represent ,! 

oi an ° p,im ' 2a,to " para ™ ,er ma,, “ The un ^ e «*■«» 
rm ne y mding the optimal parameter matrix for the optimization problem. 

5 0 Part i- Generation of End-effector Trajectory 

Trajectory planning for a manipulator can be conducted in joint space or task space. However in order ,o 
,«e advantage o, me redundant hinematicc. , is beneticiai to spec.,, the em-edector trajectory ,ask 


Le, me velocity, v and acceleration, v ol the end eltector be delined 


as: 


v « r 
v~ r 


consider the motion ol an end-eltecto, moving Irom a specilied stading position. n (t) to a specitied final 
Posmon. r, There are many ways to execute this motion. A simpie way is to move me end ettector 
amng a straight-iine path tba, connects me *o points. To describe mis , reject, the time histones o, the 

:r;r apd acceie,aiton oi ,h * * - -««*• - » » ~ 

described by smooth and simple fucntions. V 


“r:::;:;:: tt ^ ,s ,o pave • - **- « - — .. <*«, 

this requirement can be used to describe the linear speed ol the end efi .cm, 
rajectory, Fudhermore. i, is a smooth , unction and can be defined by three constants , a „ c) Using 
cycloid representation, me linear speed o, the trajecto,, is given by 


v ( P ) = b (1 - cos p) 


( 3 ) 
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where p ss the parameter ( 0 s p 5 2* ) . When p - 0. the end effector is at the initial position, rj. And when 
p - 2 n, the end-effector reaches the final position, rf 

The parameter p can be related to the time of the motion by 

t - a ( p - c sin p) (jJ . 

The distance traversed by the end-effector, d can be obtained by integrating Eqn. (3) with respect to time 

d = ab[( 1+ 0.5c) p-(c + 1) sin p + 0.25 c sin 2p] (5) 

The magnitude of linear accalerafion, v is obtained by differentiating Eqns. (3) and (4) : 

bsinp 

v(p )“ a(l-ccosp) (6 ) 

The magnitude of the maximum acceleration is 

b 

Ivmaxl- a (i-c 2 ) 0.5 (6a) 

which occurs at the instant given by 

Pmax ■ cos 1 c 

The three constants a, b, and c can be determined by forcing the cycloid curve to satisfy three motion 
constraints - total distance traversed, total time of the task, and the maximum acceleration magnitude In 
other words, we can obtain these constants by solving Eqn. (6) and the following equations 

T ( total time ) - 2 n a (7) 

0 ( total distance ) H H - r, | - ab(2 + c ) rt (8) 

After these constants are determined, the position vector, r can be obtained by 
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r-r, +du 


( 9 ) 


where u is an unit vector which points from the initial position to the final position and d is defined in Eqn 

( 5 ). 

Similarly, the velocity and acceleration of the end-effector are 

V - V u 
v- v u 

where v and v are given by Eqns. (3) and (6). 

6.0 Part 2 - Joint Space Solution 

Once the end-effector position, r(t) is defined, the minimization of base reactions reduces to determining 
the joint space solution, q(t) that minimizes the base reactions. 

In this section , we develope kinematics relations that are used in formulating the optimization problem for 
minimizing base reactions. Then we describe the optimization scheme. 

6.1 Redundancy Resolution 

We begin by determining the infinitesimal motion relation between the position vector, r(t) and joint 
variable vector, q(t) for a m degrees of freedom redundant manipulator. This relation will be useful in 
finding the joint space solution. For a given desired change in r(t), Arthe required change in q(t), Aq is 
given by: 


a r- J Aq (10) 

where J(q ) t R n * m is known as the Jacobian matrix. 

The corresponding velocity vector is 

v»J(q)q HD 

Differentiating Eqn (11) with respect to time, the acceleration of the end-effector is 
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v= j q + j q 

( 12 ) 

following two sets: ( ? bCMk Up ,hc vec,or « in “> the 

a) a set of n independent joint variables denoted by q n and 

b) a set of (m-n) ' redundant ■ joint variables denoted by q r Thus the joint vahahio „ K 

y Hr s me joint variable q can be written as 

-[:] 

Similarly, the Jacobian matrix, J can be partitioned as follows : 


( 13 ) 


J «Nn J r J 

where J n - a nonsingular nxn matrix corresponding to q n and 
J r - a submatrix nx(m-n) matrix corresponding to q r . 

Qr -s arbitrarily chosen so that J n is always invertible (nonsingular). Substituting 
Eqns (10), (t 1 ), and (12), they become: 


( 14 ) 


partitioned J and q in 


« J n Aq n + J r Aq r 
v * J n<in+ J r q r 
v - Jn Qn ♦ J r Qr + J n q n ♦ J r q r 


Rearrange Eqns. (15-17) we have 


( 15 ) 

( 16 ) 
( 17 ) 


^n “ J n _1 (Ar - J r Aq r ) 

^n- Jn ' 1 ( v - J r q r ) 


Eqns. 08-20) express me molion ol me nonredpnoan, jornt .anal 


( 18 ) 

( 19 ) 

( 20 ) 

in terms of the motion of the 
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ff d 


redundant joint variables, qr . 

6.2 Optimization Problem Formulation 

The end-effector trajectory can be divided into a number of segments. For conveniece, assume that the 
duration of each step is the same even though this assumption ;s not essential to the scheme described 
below. 

The joint trajectory at the end of an arbitrary step can be expressed as : 

P Aqn“| 

q = qo + |_AqrJ ( 21 ) 

in which q 0 = the value of q at the start of the time step. 

Substituting Eqn. (18) in Eqn. (21), we have 


q=qo + 


Jn-“ 

1 

- Jn Jr 

_ 0 _ 

0 Ar + 

_ l _ 


0** 


(22) 


Note that the subscript ( ] o denotes quantities evaluated at the known starling point (q 0 ) of the step and I 
denotes an identity matrix of size (m-n) x(m-n). 


For the purpose of optimization, we express the rates of change of redundant joint variables, qn as 

drl — ci i <i(t) + ci2f2(t) + + cik*k(t) 

qr2 » C21 fi(t) + C22 *2(t) + + C2k <k(t) 

dr(m-n) * C(m-n)1 fl(‘) + C(m-n)2 *2(t) + • ■ ■+ C(m-n)k *k(t) (23) 
where cjj =* optimization parameters 

fj = shape functions. 


Orinma:.; form, 


i 

5 "! 

i 
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q r = C f(t) 

where C is the matrix of optimization parameters, Cjj and 
f(t) is the column vector of shape functions. 


For convenience, polynomials in t are chosen for f(t) 

f(t) = [1 t t 2 1 3 . . t k ] T 
where k is the degree of the polynomial. 


Substituting Eqn. (24) in Eqns. (18 - 20), we have 



fln “ J n 1 ( v - Jr C f ) 

*in * Jn ^ I v - JfC f - J n J n '^ ( v - J r C f )- JfC f ] 


(24) 


(25) 


(26) 

(27) 

(28) 


Now, we can describe the motion of all the joint variables, q(t) by specifying the values of C matrix. For a 
particular manipulator configuration, the base reactions (F 0 and N 0 ) for the end of that time step are 

dependent on the impending motions of all the joints which by Eqns. (24 - 28) are functions of the C 
matrix. 


Recursive Newton-Euler dynamics formulation [9] is employed to obtain the base reactions. It consists of a 
set of forward and backward recursive equations. The recursive forward equations are used to compute 
linear velocity and acceleration, angular velocity and acceleration of each joint variable, proceeding from 
joint 1 to joint m. The backward recursive equations are used to compute the forces and moments exerted 
on each joint, starting from joint m and back to joint 1. Once the moments and forces exerted on joint 1 are 
computed, the base reactions (F 0 and N 0 ) can then be determined. 


6.3 Cost Function 

The problem of minimizing the magnitude of the base reactions is essentially one of minimizing a suitable 
cost function, B which is a measure of the base reactions F 0 and N 0 . 


1 0 


in general, the magnitude of base reaction force is given by 

F 0 2 a F 0l 2 + F 02 2 + F o3, (?"' 

and the magnitude of base reaction moment is 

N o 2 ■ N o1 2 + n o2 2 + n o3 2 (30) 

We introduce two weighting factors ( w 1 and w 2 ) that enable us to place different weights on base force 

component and base moment components. Now, we can define a cost fucntion , B that is a measure of 
the base reactions, 


B ■ w-f F q 2 + w 2 Nq2 

Or in matrix form, 


B-[F 0 No) 

1“ w 1* 

°1 


Lo 

•*&! J 

If we choose R - [ F 0 Nq ] T and Q * 

Wf 1 


the cost fucntion B becomes 

_0 

w 2 1 J 


(31) 


tslOle: The weights w 1 and w 2 must be carefully chosen to: 

(a) appropriately scale the base reactions with respect to a reference set of base 
reactions and 

(b) ensure that all terms in the cost function are dimensionally homogeneous. 

In the example of Section 8, we demonstrate one way of choosing w t and w 2 to satisfy the above 
requirements. 


6.4 Optimization Scheme 



lit 

k*j< 
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The optimization problem for minimizing the magnitude of the base reactions is the following: determine 
the optimization parameter matrix C which minimizes B-R^QR. The above optimization problem is solved 
using the following scheme: 

1 . Divide the end-effector trajectory r(t) into a sufficiently large number of 
segments. 

2. For a known initial configuration of the manipulator, obtain C that will minimize 
the cost fucntion B at the end point of the segment. 

3. Compute the joint trajectory ever this segment using Eqn. (26). 

4. Go to the next segment of the trajectory and repeat steps 2 and 3 . Stop if the 
end of the trajectory is reached. 

6.5 Optimization Technique 

In step 2 of the optimization scheme, an unconstrained optimization technique, the Hooke & Jeeves 
approach [12J is used to obtain the optimal C for each time step. This algorithm is one of the earliest and 
most succesful direct search methods. It does not require derivatives as in more sophiscated first-order 
methods. The disadvantage is that it has difficulties with highly constrained problems. 

7.0 Algorithm of the Approach 

An algorithm of the approach described in sections 5 and 6 and a computer program written in Pascal have 
been developed to implement the methodology. The flowchart in Figure 2 illustrates the basic algorithm. 

The first step of the approach is to find the constants of cycloid curve based on the three motion 
constraints as outlined in section 5. Thsn we formulate the optimization problem using the equations 
developed in previous section. The optimization scheme of Section 6.4 is applied to obtain a unique joint 
space solution which minimizes the base reactions. 


8.0 illustrative Example 

In this section we demonstrate how our approach can be applied to minimize the base reactions of a four 
degrees of freedom spatial manipulator proposed by NASA for space applications (see Figure 3). For a 
point-to-point spatial motion, three degrees of freedom are required. Therefore, this manipulator has one 
degree of redundancy. The manipulator has three links with lengths of /j, /^, and I3 respectively. The 

reference frame is located at the base. Link 1 is mounted to the supporting structure and the other 

two links are each driven by a differential drive mechanism (a traction drive) which has two outputs that 
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Figure 2. Flowchart of the Algorithm of Trajectory Plann 

















rotate about orthogonal axes. For the purposes of kinematic and dynamic analyses, this differential drive 
mechanism can be considered as two intersecting revolute joints. 

8.1 End-errector Trajectory 

For the end-effector trajectory, the following task specifications are chosen: 


T - 2.0s 

Ivmaxl = 20 m/s 2 

r i = [1.5m 0.5m 0.5m ] T 

r f * [1.2 m -0.5m 0.2m ] ^ 

D = | r f - rj | -i.086m. 

Solving Eqns. (6-8), the parameters for cycloid trajectory are obtained: 
a - 0.3183 
b -0.39 
C * 0.79. 


8.2 Cost Function 

To minimize the base reactions properly, it is important to select a suitable weighting matrix, Q for the cost 
fucntion, B«R T QR. Therefore, the choices of the values for the two weighting factors (w, and w 2 ) are 

crucial. To understand the effects of these weighting factors on the base reactions, three cases with 
different Q matrices are studied: 


8.21 Case 1 

In this case, we want to find out to what extent the base force component can be minimized by ignoring the 
base moment component i. e. and The cost function, B is given by 

B * F o 2 (33) 

or in matrix form, 


B-R t Q, R 


(34) 


where 



(35) 
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8.22 Case 2 


In this case we select a cost (unction that only minimizes base moment component to study how small the 
base moment can be obtained. The cost function takes the following form: 

B-N 0 2 

The weighting matrix becomes 

02= I” 0 

L 0 

8.23 Case 3 

In general, we want to minimize both the base force, F 0 and the base moment, N 0 transmitted to the base. 

Since F 0 and N 0 are different physical quantities, we require a weighting matrix Q to non-dimensionalize 

and to appropriately scale the base force and base moment. The results of Cases 1 and 2 are useful in this 
regard. 



The average value of the force ( F Q 2) transmitted to the base in Case 1 , F avg 2 is an indicator of small we 
can reduce the base force. Similarly, the average value of the moment transmitted to the base in Case 2. 
N avg 2 measures how small we can reduce the base moment. We therefore choose, 


B 


f o 2 1 *avg 2 


+ N 0 2 / N avg 2. 


The weighting matrix is given by 


(38) 


03 ■ r wi 1 ° i 09) 

L 0 w 2 1 J 

where w r - 1/F avg 2 „ average of F 0 T F 0 in Case land, 
w 2 - 1 ' Navg 2 = average of N 0 T N 0 in Case 2. 

Note that the above choices ol w, and w* simultaneously achieve Ihe desired scaling and 
non-dimensionalization. 
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8.3 Optimization Scheme 

Using the methodology developed in section 6 we can formulate the base reactions optimization problem 
for the example using the following procedures: 

1 . Select q-f as the redundant joint variable. 

qr“dl (40) 

qn = [q2n3] T < 41 > 

Using Eqn. (24) we can represent the rate of change of joint 1 by 

di = c 0 + Cl t (42) 

2. Then partition J into J n and J r (see Appendix for detailed expressions) 

3. Substituting q r in Eqns (26-28) we have a complete description of the joint 
trajectories. 

4. Apply the optimization scheme outlined in section 6 with At * 0.1s. 

8.4 Discussion of Results 

The magnitudes of the base reactions corresponding to the above three cost functions are illustrated in 
Figures 4 and 5. In Figure 4 the base forces for all three cases are shown. The base force in Case 1 is the 
smallest among the three cases. This is expected as in Case 1 only the magnitude of the base force is 
minimized. In Case 2 where only moment component is weighted, we have the largest base force. The 
magnitude of base force in Case 3 (where we minimize the scaled and weighted sum of F 0 and N 0 ) lies in 
between Cases 1 and 2. 

Figure 5 shows the base moment for all three cases. For case 1 , we have a peak in base moment at t» 
1 .2s. But in Cases 2 and 3 better results are observed. This indicates that the cost function must include a 
suitably weighted base moment. 

From results of these three different cnse studies, the cost function that weights both the base force and 
base moment is obviously a suitable cnoice for properly minimizing base reactions. The results of Figures 4 
and 5 also indicate that the weights wi and v .'2 given by Eqn. (39) are good choices for minimization of B. 

The end-effector trajectory generated using cycloid curve is shown in Figures 6-8. In Figure 9, the time 
history of the redundant joint variable, q-| is also shown. Note the joint trajectory obtained using our 



effector Trajectory 







algorithm is smooth. 


9.0 Conclusions 

In this paper, we have shown how kinematic redundancy can be employed in planning join, trajectories to 
minimize the base reactions evened by the manipulator on the supporting space structure. Using the 
approach proposed tn this paper the join, trajectories are obtained by minimizing a quadra, ic cos, tunc, ion 
which ,s a measure ot the magnitude o, the base reactions. From the results obtained, we observe tha, 

“ ” hiCh *^ h,S *' h ,be b “* •"» - base ™™n, as given by Egns. pw,, is most 
suitable for minimizing the base reactions 


The ma,or advantage o, this approach is that the cos, lunction can either be an analytical expression ,i e 
explicit) or computed trom other formulations e.g„ Newton-Euler dynamo Fomtulations ( i,e. implicit) 

The disadvantage of this approach is that it is computationally intensive and hence time consuming 
because an optimize, ion routine is required to tied ,h, optima, join, trajectories. One poss,bte solution ,s 
to use an optimization technique with a faster convergence rate. 

We have posed an optimization problem which minimizes the cost function 

B-R t QR 

a, every instant o, „me » is probably usetul to compare the -sen, approach with the use ol an - integral- 
cost function, for example ® 

B - / R T Q R dt 

avloe 7 m r ° f ,n,ereSt ln ,he Ca$e °' ,he ” in,69rar C0St ,unc,i0n we W0uld be the 

end JTre We also «> 'he ettec, o, the choice o, the 

« meet 7 ^ ' hC ,h<> P "’ M ™ *“* ** “»<>» a <**>* as 

rt meets all our motion constraints. 

7ZT We h " e ,he *“** e redundancy to solve the dynamic 

problem of minimizing base reactions. 
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Appendix : Kinematics Analysis 


The manipulator of interest >s shown in Figure 3. Links 2 and 3 are each driven by a 2 DOF traction drive 
joint. The coordinate frame X4Y4Z4 represents the position and orientation of the end effector The 
coordinate systems in Figure 3 are assigned according to Denavit-hartenberg notation [10]. The axis 
about which a link rotates is defined as the z-axis and the corresponding joint variable is q An A f '4/ 
matrix is used to describe the relative position and orientation between two link coordinate frames The 
position and orientation of the end effector with respect to the base coordinate frame described by the 
matrix T4 which is given by 


T 4 - Ao Altai) A2(q2) A 3 (q 3 ) A 4 (q 4 ) (A 1 ) 



where qi, q2, q3, and q4 are the joint variables of joint 1 *c joint 4 respectively, 

[n o a] - orientation matrix of the X4Y4Z4 with respect to the base coordinate frame XpYbZb It is the 
upper left 3x3 partitioned matrix of T4, and 

p« position vector which points from the origin of the base coordinate frame to the origin of the 
X4Y4Z4 frame. 

The following shorthand notations of sine and cosine of joint variables will be used. 

Si - sin ( qj ), Ci - cos ( qj ), Sjj « sin (qj + q), Cjj - cos (qi + q,) 

It can be shown that for this manipulator, the homogeneous transformation matrices ( Aj. i=0,l , ,4) are 


Aq - 


1 

0 

0 

0 


0 0 
1 0 
0 1 
0 0 


h 

0 

0 

1 


(A 2) 
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where 



n x 

Ox 

a x 

Pxl 

I 4 - 

n y 

Oy 

a y 

1 

p y 


! n z 

o z 

a z 

Pz 


0 

0 

0 

1 


n x ■ Ci(C 2 Cj C4 + S 2 S4) - Si S3 C4 
n y * Si (C 2 C3 C4 + S 2 S4) + Ci S3 C4 
n z = -S 2 C3 C4 + C2 S4 
Ox = Ci(S 2 C4 - C3 C 2 S4) + Si S3 S4 
Oy - Si (S 2 C 4 - C3 C? S4) - Cl S3 S 4 
°z * s 2 C3 S4 + C 2 C4 
ax =* Ci C 2 S3 + S1C3 
ay-SiC 2 S3 -C1C3 


a z ■ -S 2 S3 

p x - Ci[/3 (C 2 C3 C4 + S 2 S4) + 12 c 2 ] - 13 C4 Si + li 
p y * Si[/3 (C2 C3 C4 + S 2 S4) + /2C2] + /3C4 S3 Ci 
Pz - , 3 (C 2 S4 -S2 C3 C4) - 12 S 2 
If joint 1 is chosen to be the redundant joint, we have 




(A.7) 


and the partitioned Jacobian matrices J n and J r are 

C 2 S 4 -C 1 S2C 3 C4)-L 2 C 1 S 2 */3(CiC2S3C4+SiC3C4) 
Jn- / 3(SlC 2 S4-SiS 2 C3C4)-L2SiS2 /3VC1C3C4-S1C2S3C4) 

_-/5(S2S4+C2C3C4)-L2C 2 /3S2S3C4 
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/3<CiS2C4-ClC2C3S4+SlS3S4) 
'3(S|S2C4*SiC2C334-CiS334; 
W C2C4+S2C3S4) 


Jr* 


•l3( Si C2C3C4+S1 S2S4+C1S3C4W2S1 C2 
/j(C 1 C2C3C4+C1 S2S4-S1 S3C4)+/2Ci C2 
0 
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